#include <cassert>
#include <string.h>
#include "packet_msg_.h"
namespace acl
{
namespace mqtt
{

	std::string packet_msg (const connect_ack_msg &msg)
	{
		std::string data;
		data.resize (4);
		unsigned char* ptr = (unsigned char*)(data.data());
		ptr[0] = e_connect_ack;
		ptr[1] = 0x02;
		ptr[2] = msg.session_present_flag_ &0x01;
		ptr[3] = msg.return_code_;
		return data;
	}

	std::string packet_msg (const subscribe_ack_msg &msg)
	{
		std::string data;
		data.resize (msg.fixed_header_.remaining_length_ + 4);
		unsigned char* ptr = (unsigned char*)(data.data ());

		ptr[0] = e_subscribe_ack;
		auto p = encode_varint32(ptr + 1,msg.fixed_header_.remaining_length_);
		auto offset = p - data.data();

		ptr[offset] = msg.packet_identifier_ >> 8;
		ptr[offset+1] = msg.packet_identifier_ &0x00ff;
		offset += 2;

		for (auto &it : msg.return_code_) {
			ptr[offset] = it;
			++offset;
		}
		data.resize(offset);
		return data;
	}

	std::string packet_msg (const publish_msg & msg)
	{
		std::string data;
		auto plen = msg.payload_? msg.payload_->size():0;
		data.resize (msg.topic_name_.size () + plen + 9);

		unsigned char* ptr = (unsigned char*)(data.data ());

		ptr[0] = e_publish;
		ptr[0] |= msg.fixed_header_.DUP_flag_;
		ptr[0] |= msg.fixed_header_.QoS_level_ << 1;
		ptr[0] |= msg.fixed_header_.RETAIN_; 

		auto p = encode_varint32 (ptr + 1,msg.fixed_header_.remaining_length_);
		auto offset = p - data.data ();
		auto topic_len = msg.topic_name_.size();
		ptr[offset] = (uint8_t)(topic_len >> 8);//disable warming
		ptr[offset + 1] = topic_len & 0x00ff;
		offset += 2;
		memcpy(ptr + offset, msg.topic_name_.data(), topic_len);
		offset += topic_len;
		if (msg.fixed_header_.QoS_level_) {
			ptr[offset] = msg.packet_identifier_ >> 8;
			ptr[offset + 1] = msg.packet_identifier_ & 0x00ff;
			offset += 2;
		}
		if (plen) {
			memcpy (ptr + offset, msg.payload_->data (), plen);
			offset += plen;
		}
		assert ((int)data.size () >= offset);
		data.resize (offset);
		return data;
	}

	std::string packet_msg (const ping_rsp_msg & /* msg */)
	{
		std::string data;
		data.resize (2);
		unsigned char* ptr = (unsigned char*)(data.data ());
		ptr[0] = e_ping_rsp;
		ptr[1] = 0;
		return data;
	}

	std::string packet_msg (const publish_ack_msg &msg)
	{
		std::string data;
		data.resize (4);
		unsigned char* ptr = (unsigned char*)(data.data ());
		ptr[0] = e_publish_ack;
		ptr[1] = 0x02;
		ptr[2] = msg.packet_identifier_ >> 8;
		ptr[3] = msg.packet_identifier_ & 0x00ff;
		return data;
	}

	std::string packet_msg (const publish_rel_msg &msg)
	{
		std::string data;
		data.resize (4);
		unsigned char* ptr = (unsigned char*)(data.data ());
		ptr[0] = e_publish_rel | 0x02;
		ptr[1] = 0x02;
		ptr[2] = msg.packet_identifier_ >> 8;
		ptr[3] = msg.packet_identifier_ & 0x00ff;
		return data;
	}

	std::string packet_msg (const publish_comp_msg &msg)
	{
		std::string data;
		data.resize (4);
		unsigned char* ptr = (unsigned char*)(data.data ());
		ptr[0] = e_publish_comp;
		ptr[1] = 0x02;
		ptr[2] = msg.packet_identifier_ >> 8;
		ptr[3] = msg.packet_identifier_ & 0x00ff;
		return data;
	}

	std::string packet_msg (const unsubscribe_ack_msg &msg)
	{
		std::string data;
		data.resize (4);
		unsigned char* ptr = (unsigned char*)(data.data ());
		ptr[0] = e_unsubscribe_ack;
		ptr[1] = 0x02;
		ptr[2] = msg.packet_identifier_ >> 8;
		ptr[3] = msg.packet_identifier_ & 0x00ff;
		return data;
	}

	std::string packet_msg (const publish_rec_msg &msg)
	{
		std::string data;
		data.resize (4);
		unsigned char* ptr = (unsigned char*)(data.data ());
		ptr[0] = e_publish_rec;
		ptr[1] = 0x02;
		ptr[2] = msg.packet_identifier_ >> 8;
		ptr[3] = msg.packet_identifier_ & 0x00ff;
		return data;
	}

	char* encode_varint32 (unsigned char* dst, uint32_t v)
	{
		static const int B = 0x80;
		if (v < (1 << 7)) {
			*(dst++) = v;
		}
		else if (v < (1 << 14)) {
			*(dst++) = v | B;
			*(dst++) = v >> 7;
		}
		else if (v < (1 << 21)) {
			*(dst++) = v | B;
			*(dst++) = (v >> 7) | B;
			*(dst++) = v >> 14;
		}
		else if (v < (1 << 28)) {
			*(dst++) = v | B;
			*(dst++) = (v >> 7) | B;
			*(dst++) = (v >> 14) | B;
			*(dst++) = v >> 21;
		}
		else
			return (char *)dst;//too big

		return reinterpret_cast<char*>(dst);
	}

}
}

